//practice ia 12-13
//#include <sqlite3.h>
//#include "time_utils.cpp"
// #include <queue>
// #include <stack>
#include <map>
#include <list>
#include <time.h>
#include <math.h>
//#include <stdio.h>
#include <string.h>

#include "image_utils.cpp"
#include "nodo.h"


using namespace std;
using namespace ImageUtils;
//using namespace TimeUtils;

void Entrada(int &estrategia, int &cota, int &aumento_cota, int &num_itera){
  do{
    cout << "Estrategia:" << endl;
    cout << " (1)Anchura\n (2)Coste Uniforme \n (3)Profundidad\n";
    cout << " (4)A* Euclídea \n (5)A* Manhattan\n";
    cout << " (6)Profundidad acotada\n (7)Profundidad iterativa\n $: ";
    cin >> estrategia;
    switch(estrategia){
    case 1:
      cota = 0;
      aumento_cota = 0;
      num_itera = 1;
      break;
    case 2:
      cota = 0;
      aumento_cota = 0;
      num_itera = 1;
      break;
    case 3:
      cota = 0;
      aumento_cota = 0;
      num_itera = 1;
      break;
    case 4:
      cota = 0;
      aumento_cota = 0;
      num_itera = 1;
      break;
    case 5:
      cota = 0;
      aumento_cota = 0;
      num_itera = 1;
      break;
    case 6:
      cout << " Cota: ";
      cin >> cota;
      aumento_cota = 0;
      num_itera = 1;
      break;
    case 7:
      cout << " Cota: ";
      cin >> cota;
      cout << " Aumento de la cota: ";
      cin >> aumento_cota;
      cout << " Numero de iteraciones: ";
      cin >> num_itera;
      break;

    default:
      cout << "Opcion incorrecta" << endl;
      return;
    }
  }while((0 > estrategia)||(estrategia > 7));

}

float F_Anchura(Nodo &nodo_aux, int* posFin){
  return nodo_aux.get_prof();
}
float F_CosteU(Nodo &nodo_aux, int* posFin){
    return nodo_aux.get_cost();
}
float F_Prof(Nodo &nodo_aux, int* posFin){
  return -(nodo_aux.get_prof());
}
float F_Euclidea(Nodo &nodo_aux, int* posFin){
  float dist, cost;
  dist = sqrt(pow((posFin[0] - nodo_aux.get_coX()),2) +
	      pow((posFin[1] - nodo_aux.get_coY()),2));
  cost = nodo_aux.get_cost() + dist;
  return cost;
}
float F_Manhattan(Nodo &nodo_aux, int* posFin){
  float dist, cost;
  dist = abs(posFin[0] - nodo_aux.get_coX()) +
         abs(posFin[1] - nodo_aux.get_coY());
  cost = nodo_aux.get_cost() + dist;
  return cost;
}


bool Es_Solucion(Nodo &nodo_act, int* posFin, int** map_sol, clock_t &fin_busq){
  bool salida = false;
  // clock_t t_calc;
  if (nodo_act.get_coX() == posFin[0] &&
      nodo_act.get_coY() == posFin[1]){
    fin_busq = clock();
    nodo_act.toString();
    Nodo* nodo_aux = &nodo_act;
    cout << "\n solucion encontrada, calculando solucion" << endl;
    while(nodo_aux->get_father() != NULL){
      map_sol[nodo_aux->get_coX()][nodo_aux->get_coY()] = -1;
      nodo_aux = nodo_aux->get_father();
    }
    map_sol[nodo_aux->get_coX()][nodo_aux->get_coY()] = -1;

    //    t_calc = clock()- fin_busq;
    //    cout << "tiempo en calcular solución: " << (((float)t_calc)/CLOCKS_PER_SEC) << endl;
    BasicImageUtils::PrintMap(map_sol, posFin[0]+1, posFin[1]+1);

    // for (int i = 0; i<posFin[1]+1; i++)
    //   for (int j = 0; j<posFin[0]+1; j++)
    // 	cout << map_sol[j][i];
    salida = true;

  }
  return salida;
}

bool Busqueda(int **map, int *posIni, int *posFin, int estrategia,
	      int cota, int aumento_cota, int num_itera){
  int X,Y,i, aux_coste, posicion = 0;
  int nodos_cont = 0;
  float f =0;
  bool encontrada_sol = false;
  clock_t ini, fin = 0;


  ini = clock();

  int** map_sol = new int*[posFin[0]+1];
  for (i = 0; i <posFin[0]+1; i++) {
    map_sol[i] = new int[posFin[1]+1];
  }
  int mat2[posFin[0]+1][posFin[1]+1];
  int dep,wid;
  for(dep=0;dep<posFin[1]+1;dep++){
    //map_sol[dep] = map[dep];
    for(wid=0;wid<posFin[0]+1;wid++){
      mat2[wid][dep] = map[wid][dep];
    }
  }
  for(dep=0;dep<posFin[1]+1;dep++){
    //map_sol[dep] = map[dep];
    for(wid=0;wid<posFin[0]+1;wid++){
      map_sol[wid][dep] = mat2[wid][dep];
    }
  }

 // for (int i = 0; i<posFin[1]+1; i++)
 //      for (int j = 0; j<posFin[0]+1; j++)
 //    	cout << map_sol[j][i];


  Nodo inicial(posIni[0],posIni[1],0,0,NULL,0);
  //Nodo nodo_actual;
  float(*p_func)(Nodo&, int*);
  multimap<int,Nodo*> frontera;
  multimap<int,Nodo*> visitados;
  frontera.insert(pair<float,Nodo*>(inicial.get_f(), &inicial));
  visitados.insert(pair<int,Nodo*>(0, &inicial));

  switch(estrategia){
  case 1: // anchura
    p_func = F_Anchura;
    break;
  case 2: // coste uniforme
    p_func = F_CosteU;
    break;
  case 3: // profundidad
    p_func = F_Prof;
    break;
  case 4: // A* Euclídea
    p_func = F_Euclidea;
    break;
  case 5: // A* Manhattan
    p_func = F_Manhattan;
    break;
  case 6: // profundidad acotada
    p_func = F_Prof;
    break;
  case 7: // profundidad iterativa
    p_func = F_Prof;
    break;
  default:
    cout << "Opcion incorrecta en P_FUNC!!!!" << endl;
    return false;
  }


    while (!frontera.empty()) {
      Nodo* nodo_actual = frontera.begin()->second;
      frontera.erase(frontera.begin());
      encontrada_sol = Es_Solucion(*nodo_actual, posFin, map_sol, fin);
      if (encontrada_sol)
	break;
      else{ //expandir
	if ((estrategia < 6)||(estrategia == 7 && nodo_actual->get_prof() < cota)){
	  // //////////////////// PEPE EL GUARRO ////////////////////
	  // cout << "nodo_actual: " << nodo_actual << " ";
	  // nodo_actual->toString();
	  // ////////////////////////////////////////////////////////

	  X = nodo_actual->get_coX();
	  Y = nodo_actual->get_coY();

	  /* expandimos  DCHA-ABAJO-IZQ-ARRIBA */
	  if (X+1 <= posFin[0]){
	    if (map_sol[X+1][Y]==0)
	      aux_coste = 100;
	    else
	      aux_coste = map_sol[X+1][Y];

	    Nodo * aux_nodo = new Nodo(0,0,0,0,NULL,0);
	    aux_nodo->set(X+1,Y,aux_coste,nodo_actual);
	    f = p_func(*aux_nodo, posFin);
	    posicion = X+1+(Y*(posFin[0]+1));
	    if (map[X+1][Y]<0){
	      Nodo* nodo_ant = visitados.find(posicion)->second;
	      if (aux_nodo->get_cost() < nodo_ant->get_cost()){
		aux_nodo->set_f(f);
		frontera.insert(pair<int,Nodo*>(aux_nodo->get_f(),aux_nodo));
		visitados.insert(pair<int,Nodo*>(posicion,aux_nodo));
		nodos_cont ++;
	      }
	    }
	    else{
	      aux_nodo->set_f(f);
	      frontera.insert(pair<int,Nodo*>(aux_nodo->get_f(),aux_nodo));
	      map[X+1][Y] = -1;
	      visitados.insert(pair<int,Nodo*>(posicion,aux_nodo));
	      nodos_cont ++;
	    }

	    // //////////////////// PEPE EL GUARRO ////////////////////
	    // cout << "aux nodo1: " << aux_nodo << " ";
	    // aux_nodo->toString();
	    // ////////////////////////////////////////////////////////
	  }
	  if (Y+1 <= posFin[1]){
	    if (map_sol[X][Y+1]==0)
	      aux_coste = 100;
	    else
	      aux_coste = map_sol[X][Y+1];

	    Nodo * aux_nodo = new Nodo(0,0,0,0,NULL,0);
	    aux_nodo->set(X,Y+1,aux_coste,nodo_actual);
	    f = p_func(*aux_nodo, posFin);
	    posicion = X+((Y+1)*(posFin[0]+1));
	    if (map[X][Y+1]<0){
	      Nodo* nodo_ant = visitados.find(posicion)->second;
	      if (aux_nodo->get_cost() < nodo_ant->get_cost()){
		aux_nodo->set_f(f);
		frontera.insert(pair<int,Nodo*>(aux_nodo->get_f(),aux_nodo));
		visitados.insert(pair<int,Nodo*>(posicion,aux_nodo));
		nodos_cont ++;
	      }
	      // else{
	      // 	delete nodo_ant;
	      // 	delete aux_nodo;
	      // }
	    }
	    else{
	      aux_nodo->set_f(f);
	      frontera.insert(pair<int,Nodo*>(aux_nodo->get_f(),aux_nodo));
	      map[X][Y+1] = -1;
	      visitados.insert(pair<int,Nodo*>(posicion,aux_nodo));
	      nodos_cont ++;
	    }


	    // //////////////////// PEPE EL GUARRO ////////////////////
	    // cout << "aux nodo2: " << aux_nodo << " ";
	    // aux_nodo->toString();
	    // ////////////////////////////////////////////////////////
	  }
	  if (X-1 >= 0){// && map[X-1][Y]>=0){
	    if (map_sol[X-1][Y]==0)
	      aux_coste = 100;
	    else
	      aux_coste = map_sol[X-1][Y];

	    Nodo * aux_nodo = new Nodo(0,0,0,0,NULL,0);
	    aux_nodo->set(X-1,Y,aux_coste,nodo_actual);
	    f = p_func(*aux_nodo, posFin);
	    posicion = X-1+(Y*(posFin[0]+1));
	    if (map[X-1][Y]<0){
	      Nodo* nodo_ant = visitados.find(posicion)->second;
	      if (aux_nodo->get_cost() < nodo_ant->get_cost()){
		aux_nodo->set_f(f);
		frontera.insert(pair<int,Nodo*>(aux_nodo->get_f(),aux_nodo));
		visitados.insert(pair<int,Nodo*>(posicion,aux_nodo));
		nodos_cont ++;
	      }
	      // else{
	      // 	delete nodo_ant;
	      // 	delete aux_nodo;
	      // }
	    }
	    else{
	      aux_nodo->set_f(f);
	      frontera.insert(pair<int,Nodo*>(aux_nodo->get_f(),aux_nodo));
	      map[X-1][Y] = -1;
	      visitados.insert(pair<int,Nodo*>(posicion,aux_nodo));
	      nodos_cont ++;
	    }
	    // //////////////////// PEPE EL GUARRO ////////////////////
	    // cout << "aux nodo3: " << aux_nodo << " ";
	    // aux_nodo->toString();
	    // ////////////////////////////////////////////////////////
	  }
	  if (Y-1 >= 0){
	    if (map_sol[X][Y-1]==0)
	      aux_coste = 100;
	    else
	      aux_coste = map_sol[X][Y-1];

	    Nodo * aux_nodo = new Nodo(0,0,0,0,NULL,0);
	    aux_nodo->set(X,Y-1,aux_coste,nodo_actual);
	    f = p_func(*aux_nodo, posFin);
	    posicion = X+((Y-1)*(posFin[0]+1));
	    if (map[X][Y-1]<0){
	      Nodo* nodo_ant = visitados.find(posicion)->second;
	      if (aux_nodo->get_cost() < nodo_ant->get_cost()){
		aux_nodo->set_f(f);
		frontera.insert(pair<int,Nodo*>(aux_nodo->get_f(),aux_nodo));
		visitados.insert(pair<int,Nodo*>(posicion,aux_nodo));
		nodos_cont ++;
	      }
	      // else{
	      // 	delete nodo_ant;
	      // 	delete aux_nodo;
	      // }
	    }
	    else{
	      aux_nodo->set_f(f);
	      frontera.insert(pair<int,Nodo*>(aux_nodo->get_f(),aux_nodo));
	      map[X][Y-1] = -1;
	      visitados.insert(pair<int,Nodo*>(posicion,aux_nodo));
	      nodos_cont ++;
	    }
	    // //////////////////// PEPE EL GUARRO ////////////////////
	    // cout << "aux nodo4: " << aux_nodo << " ";
	    // aux_nodo->toString();
	    // ////////////////////////////////////////////////////////
	  }
	}// if estrategia...
      }// else Es_Solucion
    }// while
    if (encontrada_sol){
      cout << "Nodos expandidos: " << nodos_cont<< endl;
      fin = fin - ini;
      cout << "Tiempo de busqueda: " << (((float)fin)/CLOCKS_PER_SEC) << endl;
    }

  return encontrada_sol;
}// Busqueda



int main (){
  int width,depth,i;
  bool sol_encontrada = false;

  //  char* imagen = NULL;
  // 1 anch, 2 cost_unif, 3 prof, 4 prof_acot, 5 prof_iterativa
  int estrategia = 0;
  int cota = 0;
  int aumento_cota = 0;
  int num_itera = 0;

  Entrada(estrategia, cota, aumento_cota, num_itera);

  BasicImageUtils::ImageDimensions((char*)"20x20.pgm",width,depth);

  int** map = new int*[width];
  for (i = 0; i < width; i++) {
    map[i] = new int[depth];
  }
  BasicImageUtils::Map((char *)"20x20.pgm", map);



  int* posIni = new int[2];
  posIni[0] = 0;
  posIni[1] = 0;
  int* posFin = new int[2];
  posFin[0] = width-1;
  posFin[1] = depth-1;

  int cont_itera = 0;

  clock_t ini;
  ini = clock();

  if (estrategia == 7){
    while(cont_itera <= num_itera){
      sol_encontrada = Busqueda(map, posIni, posFin, estrategia, cota, aumento_cota, num_itera);
      if (sol_encontrada)
	break;
      else{
	cota += aumento_cota;
	cont_itera++;
      }
    }
  }
  else
    sol_encontrada = Busqueda(map, posIni, posFin, estrategia, cota, aumento_cota, num_itera);

  if (!sol_encontrada)
    cout << "Solucion no encontrada" << endl;

  ini = clock() - ini;

  //  printf ("Tiempo total de ejecución: (%f seconds).\n",((float)ini)/CLOCKS_PER_SEC);
  cout << "Tiempo total de ejecución: " << (((float)ini)/CLOCKS_PER_SEC)<< endl;
  for(int i = 0; i < depth; ++i)
    delete[] map[i];
  delete[] map;

  return 0;
}



